#include "Energy.h"

module EnergyP {

	provides {
		interface Init;
		interface EnergyState;
		interface EnergyFlags;
	}
	
	uses {
		interface Leds;
		interface GeneralIO as ADC6; // Pin1
		interface GeneralIO as ADC7; // Pin2
		interface GeneralIO as ADC3; // Pin3
		interface GeneralIO as GIO0;
	}

}
implementation{

#ifndef PLATFORM_TELOSB
#error "Telosb platform specific code compiled."
#endif

	command error_t Init.init() {
		call ADC6.makeOutput();
		call ADC7.makeOutput();
		call ADC3.makeOutput();
		
		// make sure GIO0 is set to input if connected to ADC3
		call GIO0.makeInput();
		call ADC3.set();
		if(call GIO0.get()) {
			call ADC3.clr();
			if(call GIO0.get()) {
				call GIO0.makeOutput();
			}
		} else {
			call GIO0.makeOutput();
		}
		
		return SUCCESS;
	}

	inline void setPinValue(const uint8_t value) __attribute__ ((always_inline)) {
		//#define PORT6 (*TCAST(volatile TYPE_PORT_OUT* ONE, P6OUT_))
		//atomic PORT6 |= ((value & 1) << 6) |  ((value & 2) << 7) | ((value & 4) << 3);
		if(value & 1) call ADC6.set();
		else call ADC6.clr();
		
		if(value & 2) call ADC7.set();
		else call ADC7.clr();
		
		if(value & 4) call ADC3.set();
		else call ADC3.clr();
	}

	inline error_t setState(const energy_state_t state) __attribute__ ((always_inline)) {
		switch(state) {
#ifndef ENERGY_SAFE_STATE
#warning "ENERGY NOSAFE STATES USED"
			case ENERGY_STATE1:
				setPinValue(0);
				call Leds.set(0);
				return SUCCESS;
			case ENERGY_STATE2:
				setPinValue(1);
				call Leds.set(1);
				return SUCCESS;
			case ENERGY_STATE3:
				setPinValue(2);
				call Leds.set(2);
				return SUCCESS;
			case ENERGY_STATE4:
				setPinValue(3);
				call Leds.set(3);
				return SUCCESS;			
			case ENERGY_STATE5:
				setPinValue(4);
				call Leds.set(4);
				return SUCCESS;
			case ENERGY_STATE6:
				setPinValue(5);
				call Leds.set(5);
				return SUCCESS;
			case ENERGY_STATE7:
				setPinValue(6);
				call Leds.set(6);
				return SUCCESS;
			case ENERGY_STATE8:
				setPinValue(7);
				call Leds.set(7);
				return SUCCESS;
#else
			case ENERGY_STATE1:
				setPinValue(1);
				call Leds.set(1);
				return SUCCESS;
			case ENERGY_STATE2:
				setPinValue(2);
				call Leds.set(2);
				return SUCCESS;
			case ENERGY_STATE3:
				setPinValue(4);
				call Leds.set(4);
				return SUCCESS;
			case ENERGY_STATE4:
				setPinValue(7);
				call Leds.set(7);
				return SUCCESS;
#endif				
			default:
				return EINVAL;
		}
	}

	command error_t EnergyState.setState(const energy_state_t state) {
		return setState(state);
	}
	
	inline error_t setFlag(const energy_flag_t flag, const bool value) __attribute__ ((always_inline)) {
		switch(flag) {
			case ENERGY_FLAG1: 
				if(value) {
					call ADC6.set();
					call Leds.led0On();
				}  else {
					call ADC6.clr();
					call Leds.led0Off();	
				}
				return SUCCESS;
			case ENERGY_FLAG2: 
				if(value) {
					call ADC7.set();
					call Leds.led1On();	
				} else {
					call ADC7.clr();
					call Leds.led1Off();
				}
				return SUCCESS;
			case ENERGY_FLAG3: 
				if(value) {
					call ADC3.set();
					call Leds.led2On();
				}  else {
					call ADC3.clr();
					call Leds.led2Off();	
				}
				return SUCCESS;
			default:
				return EINVAL;
		}
	}
	
	command error_t EnergyFlags.setFlag(const energy_flag_t flag) {
		return setFlag(flag, TRUE);
	}
	
	command error_t EnergyFlags.clearFlag(const energy_flag_t flag) {
		return setFlag(flag, FALSE);
	}

	inline void energy_state(const energy_state_t state) @C() @spontaneous() __attribute__ ((always_inline)) {
		setState(state);
	}
	
	inline void energy_flag(const energy_flag_t flag, const bool value) @C() @spontaneous() __attribute__ ((always_inline)) {
		setFlag(flag, value);
	}

}